//
// File: desired_cordinate.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 06-Sep-2024 23:57:11
//

// Include Files
#include "desired_cordinate.h"
#include <cmath>

// Function Definitions
//
// Arguments    : double xk_1
//                double yk_1
//                double car_angle_k_1
//                double vd
//                double wd
//                double dtd
//                double *Xd
//                double *Yd
//                double *car_angle_d
// Return Type  : void
//
void desired_cordinate(double xk_1, double yk_1, double car_angle_k_1,
                       double vd, double wd, double dtd, double *Xd, double *Yd,
                       double *car_angle_d)
{
  double b_dtd[6];
  double Pk[3];
  // P更新
  b_dtd[0] = dtd * std::cos(car_angle_k_1);
  b_dtd[3] = 0.0;
  b_dtd[1] = dtd * std::sin(car_angle_k_1);
  b_dtd[4] = 0.0;
  b_dtd[2] = 0.0;
  b_dtd[5] = dtd;
  Pk[0] = xk_1;
  Pk[1] = yk_1;
  Pk[2] = car_angle_k_1;
  for (int i{0}; i < 3; i++) {
    Pk[i] += b_dtd[i] * vd + b_dtd[i + 3] * wd;
  }
  *Xd = Pk[0];
  *Yd = Pk[1];
  *car_angle_d = Pk[2];
}

//
// File trailer for desired_cordinate.cpp
//
// [EOF]
//
